﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace tamzarian
{
    public static class DualQuaternionTransformation
    {
        public static Vector3 Transfom(this DualQuaternion q, Vector3 v)
        {
            var W = q.q0.W;
            var X = q.q0.X;
            var Y = q.q0.Y;
            var Z = q.q0.Z;
            var P = q.qe.W;
            var Q = q.qe.X;
            var R = q.qe.Y;
            var S = q.qe.Z;
            var WW = W * W;
            var XX = X * X;
            var YY = Y * Y;
            var ZZ = Z * Z;
            var WX = W * X;
            var WY = W * Y;
            var WZ = W * Z;
            var XY = X * Y;
            var XZ = X * Z;
            var YZ = Y * Z;
            return new Vector3(
              2 * (Q * W - P * X - R * Z + S * Y) + (WW + XX - YY - ZZ) * v.X + 2 * (XY - WZ) * v.Y + 2 * (XZ + WY) * v.Z,
              2 * (Q * Z - P * Y + R * W - S * X) + 2 * (XY + WZ) * v.X + (WW - XX + YY - ZZ) * v.Y + 2 * (YZ - WX) * v.Z,
              2 * (R * X - P * Z + S * W - Q * Y) + 2 * (XZ - WY) * v.X + 2 * (YZ + WX) * v.Y + (WW - XX - YY + ZZ) * v.Z);
        }
    }
}
